#include "app_3axis_output.h"
#include "app.h"
#include "bluenrg_gap.h"
#include "sensor_service.h"
#include "task.h"
#include "stm32l052_lp_mode.h"
#include "eeprom.h"
#include "wdg.h"
#include "sht3x_dis.h"
#include "voltage.h"
/*private define***************************************************************************************/
#define SAMPLE_GROUP_NUM(x)   (x / 3)
/*private variable*************************************************************************************/
extern I2C_HandleTypeDef I2C_EXPBD_Handle;
volatile axis_type_t axis_buf[AXIS_BUFFER_SIZE] = {0};
volatile uint16_t sample_top = 0;
volatile uint8_t dma_over_flag = DMA_BEGIN;
uint16_t combo_data_buf[8];

/*private function*************************************************************************************/

#ifdef APP_VIBERATE_ABS
static APP_Status_t app_viberate_abs(uint32_t* data);
#endif

/*arithmatic*******************************************************************************************/
uint16_t app_int_sqrt(uint32_t data)
{
    uint32_t r = 0, t = 0;
    int8_t i = 0;
    
    for(i = 15; i >= 0; i--){
        t = r | ( 0x01 << i);
        if(((t) * (t)) <= data){
            r = t;
        }
    }
    
    return r;
}

/*average*/
static APP_Status_t app_average_arithmatic(axis_average_value_t* data)
{
    uint16_t i;
    int sum_x = 0, sum_y = 0, sum_z = 0;
 
    /*sum*/
    for(i = 0; i < sample_top ; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;    
    }
   /*average*/
   data->average_x = (int16_t)(sum_x / sample_top );
   data->average_y = (int16_t)(sum_y / sample_top );
   data->average_z = (int16_t)(sum_z / sample_top );
    
   return app_ok;

}

/*viberate*/
static APP_Status_t app_viberate_arithmatic(uint16_t* data)
{   
    axis_average_value_t average;
    uint16_t i;
    
    uint32_t sum_sq_x = 0, sum_sq_y = 0, sum_sq_z = 0, average_sq_x = 0, average_sq_y = 0, average_sq_z = 0;
 
   if(app_average_arithmatic(&average) != app_ok)
        return app_error;
   /*sum of square*/
  for(i = 0; i < sample_top; i++){
       sum_sq_x += (uint32_t)((axis_buf[i].axis_x - average.average_x) * (axis_buf[i].axis_x - average.average_x));
       sum_sq_y += (uint32_t)((axis_buf[i].axis_y - average.average_y) * (axis_buf[i].axis_y - average.average_y));
       sum_sq_z += (uint32_t)((axis_buf[i].axis_z - average.average_z) * (axis_buf[i].axis_z - average.average_z));
   }
   /*average*/
   average_sq_x = sum_sq_x / sample_top;
   average_sq_y = sum_sq_y / sample_top;
   average_sq_z = sum_sq_z / sample_top;
   /*sqrt*/
   data[0] = (uint16_t)app_int_sqrt(average_sq_x);
   data[1] = (uint16_t)app_int_sqrt(average_sq_y) ;
   data[2] = (uint16_t)app_int_sqrt(average_sq_z);
   
   return app_ok;
   
}

/*p-p value*/
static APP_Status_t app_p2p_arithmatic(axis_p2p_value_t* data)
{
    uint16_t i = 0;
    int16_t x_max = 0,x_min = 0,y_max = 0,y_min = 0,z_max = 0,z_min = 0;
  
    x_max = axis_buf[0].axis_x;
    x_min = axis_buf[0].axis_x;
 
    y_max = axis_buf[0].axis_y;
    y_min = axis_buf[0].axis_y;
 
    z_max = axis_buf[0].axis_z;
    z_min = axis_buf[0].axis_z;
 
    for(i = 1; i < sample_top; i++){
        if(axis_buf[i].axis_x > x_max)
            x_max = axis_buf[i].axis_x;
        if(axis_buf[i].axis_x < x_min)
            x_min = axis_buf[i].axis_x ;
        
        if(axis_buf[i].axis_y > y_max)
            y_max = axis_buf[i].axis_y;
        if(axis_buf[i].axis_y < y_min)
            y_min = axis_buf[i].axis_y ;
        
        if(axis_buf[i].axis_z > z_max)
            z_max = axis_buf[i].axis_z;
        if(axis_buf[i].axis_z < z_min)
            z_min = axis_buf[i].axis_z ;
         
    }
    
    data->p2p_x = x_max - x_min;
    data->p2p_y = y_max - y_min;
    data->p2p_z = z_max - z_min;
    
    
    return app_ok;
}


/*data option******************************************************************************************/

void app_dma_flag_set(uint8_t data)
{
    dma_over_flag = data;
}

uint8_t app_dma_flag_get(void)
{
    return dma_over_flag;
}

void app_3axis_int_callback(void)
{
    app_dma_flag_set(DMA_END);
}


static void app_combo_output(void)
{
    axis_p2p_value_t* p2p_data = (void*)combo_data_buf;
 
    /*p2p*/
    app_p2p_arithmatic(p2p_data);
    /*viberate*/
    app_viberate_arithmatic(&combo_data_buf[3]);  
}

static void lis2dh12_value_handle(void)
{
    app_combo_output();
}

void lis2dh12_get_config_info(lis2dh12_param_t* param_data)
{
    param_data->full_scal = eeprom_read_half_word(EEPROM_3AXIS_FULLSCAL_ADDR);
    param_data->odr = eeprom_read_half_word(EEPROM_3AXIS_ODR_ADDR);
    param_data->sample_window = eeprom_read_half_word(EEPROM_3AXIS_SAMPLE_WINDOW_ADDR);
    param_data->sample_period = eeprom_read_half_word(EEPROM_3AXIS_PERIOD_ADDR);
   
}


static uint8_t lis2dh12_config_sensor(lis2dh12_param_t param_data)
{
     uint8_t odr = 0, full_scal = 0;
 
     switch(param_data.odr){
      
      case 0:    odr = LIS2DH12_POWER_DOWN;    break;
      case 1:    odr = LIS2DH12_ODR_1HZ;       break;
      case 10:   odr = LIS2DH12_ODR_10HZ;      break;
      case 25:   odr = LIS2DH12_ODR_25HZ;      break;
      case 50:   odr = LIS2DH12_ODR_50HZ;      break;
      case 100:  odr = LIS2DH12_ODR_100HZ;     break;
      case 200:  odr = LIS2DH12_ODR_200HZ;     break;
      case 400:  odr = LIS2DH12_ODR_400HZ;     break;
      case 1620: odr = LIS2DH12_ODR_LP_1620HZ; break;
      case 5376: odr = LIS2DH12_ODR_LP_5376HZ; break;
      
      default: app_fault_handle(APP_LIS2DH12_PARAM_ERROR);break;
     }
     
     switch(param_data.full_scal){
      case 0:  break;
      case 2:  full_scal = LIS2DH12_FS_2G;      break;
      case 4:  full_scal = LIS2DH12_FS_4G;      break;
      case 8:  full_scal = LIS2DH12_FS_8G;      break;
      case 16: full_scal = LIS2DH12_FS_16G;     break;
      
      default: app_fault_handle(APP_LIS2DH12_PARAM_ERROR);break;
     }
     
     /*full scale set*/
     if(LIS2DH12_FS_Set(full_scal) != APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;
     /*fifo enable*/
     if(LIS2DH12_FIFO_Enable(LIS2DH12_FIFO_ENABLE)!= APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;
     
     /*fifo mode set*/
     if(LIS2DH12_FIFO_Mode_Set(LIS2DH12_FIFO_MODE_STREAM) != APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;   
    
     /*fifo fth int set*/
     if(LIS2DH12_FIFO_WTM_INT1_Enable(LIS2DH12_WTM_INT1_ENABLE)!= APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;
     
     /*fifo fth value set*/
     if(LIS2DH12_FIFO_FTH_Value_Set(LIS2DH12_SAMPLE_NUMBER - 1)!= APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;
      
      /*x , y, z enable*/
     if(LIS2DH12_Axis_Enable(LIS2DH12_AXIS_X_ENABLE | LIS2DH12_AXIS_Y_ENABLE | LIS2DH12_AXIS_Z_ENABLE)!= APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;
     
     if(LIS2DH12_FIFO_Buffer_Reset()!= APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 0x01;

      /*ODR set*/
     if(LIS2DH12_Data_Rate_Set(odr) != APP_LIS2DH12_OK)
         return APP_LIS2DH12_ERROR + 1;
     
     if(LIS2DH12_INT_IO_ITConfig() != APP_LIS2DH12_OK)
     return APP_LIS2DH12_ERROR + 0x01;
     
     return APP_LIS2DH12_OK;
     
    
}


/*record time stamp********************************************************************************/
static void record_time_stamp_in_eeprom(void)
{
    uint32_t time = 0;
    eeprom_data_t eeprom_data;
   
    time = get_time();
 
    eeprom_erase(1, EEPROM_DATA_TIME_STAMP_VALUE_ADD);
    
    eeprom_data.address = EEPROM_DATA_TIME_STAMP_VALUE_ADD;
    eeprom_data.len = 1;
    eeprom_data.eeprom_data = &time;
 
    eeprom_write(eeprom_data);
 
}
/*process******************************************************************************************/

lis2dh12_param_t sensor_param_data ;

void lis2dh12_config(void)
{
    uint8_t tmp;
    
    tmp = lis2dh12_config_sensor(sensor_param_data);
    if(tmp != 0x00)
        app_fault_handle(tmp);
}

void lis2dh12_begin(void)
{
    lis2dh12_get_config_info(&sensor_param_data);
    
     /*init io*/
    if(LIS2DH12_Init() != APP_LIS2DH12_OK)
        app_fault_handle( APP_LIS2DH12_ERROR);
    /*read id*/
    if(app_lis2dh12_check_id() != APP_LIS2DH12_OK)
        app_fault_handle( APP_LIS2DH12_ERROR + 2);
    
    /*power mode set*/
    if(LIS2DH12_Power_Mode_Set(LIS2DH12_NORMAL_MODE) != APP_LIS2DH12_OK)
        app_fault_handle (APP_LIS2DH12_ERROR + 2);
    
     /*ODR set*/
    if(LIS2DH12_Data_Rate_Set(LIS2DH12_POWER_DOWN) != APP_LIS2DH12_OK)
        app_fault_handle(APP_LIS2DH12_ERROR + 2);
    
}

void lis2dh12_process(void)
{
    lis2dh12_value_handle();
    lis2dh12_end();     
}

void lis2dh12_end(void)
{
    record_time_stamp_in_eeprom();
    get_voltage();
}

uint8_t lis2dh12_sample_end_flag(uint16_t sample_count)
{
    uint16_t sample_max = 0;
 
    if(sample_top == 0){
        sample_max = eeprom_read_half_word(EEPROM_3AXIS_SAMPLE_WINDOW_ADDR);
        sample_top = sample_max;
        /*eeprom data block = 2k - used block = 1800bytes*/

          if(sample_top > AXIS_BUFFER_SIZE)     
              sample_top = AXIS_BUFFER_SIZE; 
    }
    
    if((sample_count ) >= sample_top){
        app_lis2dh12_powerdown();
        return SAMPLE_END;
    }
    
    return SAMPLE_ON;
}


